/* AUTHOR -> ANATOLIJS BOGDANOVS & LYUBOMIR KOZLOVSKI 
   (ALL CODE AUTHORED BY ANATOLIJS UNLESS OTHERWISE STATED) */

#include "mouse_sensor.h"

#include "lpc_types.h"
#include "KeyboardHost.h"
#include "timer.h"
#include <math.h>

/* Private defines -------------------------------------------------------- */
#define     PI          3.142
#define     a_X         70
#define     a_Y         0
#define     THETA       -0.0115 // AUTHOR -> LYUBOMIR
#define     THETA2      -0.007 // AUTHOR -> LYUBOMIR
#define     EPS         1e-6
#define     P_TO_CM     372 // AUTHOR -> LYUBOMIR
#define     BUF_SIZE    10 // AUTHOR -> LYUBOMIR

/* Private flags ---------------------------------------------------------- */
volatile uint8_t mouse_sensor_init = 0;
volatile uint8_t mouse_active = 0;
volatile uint8_t mouse_proper_active = 0;

/* Private globals -------------------------------------------------------- */
double last_x, last_y;
double dfi, dx, dy;
double total_x, total_y, total_fi;
uint8_t buffer_count;

/* Mouse events ----------------------------------------------------------- */

// AUTHOR -> LYUBOMIR
void _on_mouse_poll(uint8_t unused, int8_t X, int8_t Y)
{
    if (mouse_proper_active)
    {    
        dx += X;
        dy += Y;    
        /*  Buffer values  */
        if(buffer_count < BUF_SIZE)
        {
            ++buffer_count;
        }
        else
        {
            
            /* Scale by sensor offset angle */
            //double temp = sqrt(dx * dx + dy * dy);
            //dx = temp * sin(THETA2);
            //dy = temp * cos(THETA2);
            
            /*  Proper coordinates */
            total_x += -dy;
            total_y += dx;
            
            buffer_count = 0;
            dx = 0;
            dy = 0;
        }    
    }
    
    else if (mouse_active && (X || Y))
        mouse_proper_active = 1;
    else
        mouse_active = 0;
    return;
}
// END AUTHOR

void _on_mouse_attach()
{
    mouse_active = 1;
    return;
}

void _on_mouse_detach()
{
    mouse_active = 0;
    return;
}

/* Init functions --------------------------------------------------------- */

void init_mouse_sensors(void)
{
    if (mouse_sensor_init)
        return;
    
    mouse_init(_on_mouse_poll, _on_mouse_attach, _on_mouse_detach);
    init_timer();
    enable_timer(10, 500, mouse_poll);
    
    // Private Attribute Init
    last_x = 0;
    last_y = 0;
    total_x = 0;
    total_y = 0;
    buffer_count = 0;
    
    // Private Flags Set
    mouse_sensor_init = 1;
    mouse_active = 0;
    
    return;
}

/* Data Accessors --------------------------------------------------------- */
int32_t get_x(void)
{
    return (int32_t)total_x;
}

int32_t get_y(void)
{
    return (int32_t)total_y;
}

int32_t get_fi(void)
{
    return (int32_t)(total_fi * 180.0 / PI);
}

uint8_t get_mouse_active(void)
{
    return mouse_proper_active;
}

void reset_mouse_position(void)
{
    total_x = 0;
    total_y = 0;
    total_fi = 0;
    return;
}

int16_t get_x_cm(void)
{
    return (int16_t)(total_x / P_TO_CM);
}

int16_t get_y_cm(void)
{
    return (int16_t)(total_y / P_TO_CM);
}
